package org.beltrw.Pilot;


import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassSensor;
import lejos.robotics.navigation.TachoPilot;

/**
 * Pilot class for the Pink Robot. Utilizes all necessary motors and sensors.
 * Optimized to save power by shutting off the Ultrasonic and Light Sensors
 * when not in use.
 *
 * @author ryanbelt
 */
public class PinkRobotPilot extends TachoPilot
{
    private Motor rightMotor;
    private Motor leftMotor;
    private CompassSensor compassSensor;
    private int trackWidth;
    private int wheelDiameter;
    private boolean reverse;
    private LightSensor lightSensor;
    private UltrasonicSensor usSensor;

    /**
     * Constructor for robot with Light, Compass, and Ultrasonic Sensors with 4 wheels.
     * Based on and named after the Pink Robot.
     *
     * @param rightMotor - the right motor
     * @param leftMotor - the left motor
     * @param compassSensor - the Compass Sensor
     * @param trackWidth - distance between center of wheels
     * @param wheelDiameter - the diameter of the wheels
     * @param reverse - true if backwards is forwards and vice versa, false otherwise
     * @param lightSensor - the Light Sensor
     * @param usSensor the Ultrasonic Sensor
     */
    public PinkRobotPilot(Motor rightMotor, Motor leftMotor, CompassSensor compassSensor, int trackWidth, int wheelDiameter, boolean reverse, LightSensor lightSensor, UltrasonicSensor usSensor) {
        super(wheelDiameter, wheelDiameter, trackWidth, leftMotor, rightMotor, reverse);
        this.rightMotor = rightMotor;
        this.leftMotor = leftMotor;
        this.compassSensor = compassSensor;
        this.trackWidth = trackWidth;
        this.wheelDiameter = wheelDiameter;
        this.reverse = reverse;
        this.lightSensor = lightSensor;
        lightSensor.setFloodlight(false); // to save power
        this.usSensor = usSensor;
        usSensor.off(); // to save power
    }

    /**
     * Constructor for robot with Light, Compass, and Ultrasonic Sensors with 4 wheels.
     * Based on and named after the Pink Robot.
     *
     * @param rightMotor - the right motor
     * @param leftMotor - the left motor
     * @param compassSensor - the Compass Sensor
     * @param trackWidth - distance between center of wheels
     * @param wheelDiameter - the diameter of the wheels
     * @param lightSensor - the Light Sensor
     * @param usSensor the Ultrasonic Sensor
     */
    public PinkRobotPilot(Motor rightMotor, Motor leftMotor, CompassSensor compassSensor, int trackWidth, int wheelDiameter, LightSensor lightSensor, UltrasonicSensor usSensor) {
        this(rightMotor, leftMotor, compassSensor, trackWidth, wheelDiameter, false, lightSensor, usSensor);
    }


    

    /**
     * Method to turn by a certain number of degrees using the compass sensor
     * @param degrees
     */
    public void turn(int degrees)
    {
        compassSensor.resetCartesianZero();
        float initialValue = compassSensor.getDegreesCartesian();
        LCD.drawString("IValue = " + initialValue, 0, 0);
        rotate(degrees);
        float nextValue = compassSensor.getDegreesCartesian();
        LCD.drawString("Value = " + nextValue, 0,1);

    }

    public void turnLeft90()
    {
        compassSensor.resetCartesianZero();
        rotate(-65);
        float degree = compassSensor.getDegreesCartesian();
        leftMotor.setSpeed(50);
        rightMotor.setSpeed(50);
        leftMotor.forward();
        rightMotor.backward();
        while(degree < 90)
        {
            //rotate(-2);
            try
            {
                Thread.sleep(200);
            }catch(Exception e){}
            degree = compassSensor.getDegreesCartesian();
        }
        leftMotor.stop();
        rightMotor.stop();
        //rotate(-90);
        //rotate(compassSensor.getDegreesCartesian()-90);
    }

    public void turnRight90()
    {
        compassSensor.resetCartesianZero();
        rotate(65);
        float degree = compassSensor.getDegreesCartesian();
        leftMotor.setSpeed(50);
        rightMotor.setSpeed(50);
        rightMotor.forward();
        leftMotor.backward();
        while(degree > 270 || degree < 10)
        {
            //rotate(2);
            try
            {
                Thread.sleep(200);
            }catch(Exception e){}
            degree = compassSensor.getDegreesCartesian();
        }
        leftMotor.stop();
        rightMotor.stop();
        //rotate(90);
        //rotate(270 - compassSensor.getDegreesCartesian());
    }

    /**
     * Moves forward by 15.5 inches
     *
     * @param distance - distance from center of one cell to center of next in same units used in constructor
     */
    public void forwardACell(float distance)
    {
        travel(distance);
    }

    /**
     * Uses the ultrasonic sensor to take note of the robot's surroundings
     *
     * @return - array of readings where index 0 = right reading, 1 = forward reading, and 2 = left reading
     */
    public int[] getSurroundings(Motor head)
    {
        int[] surr = new int[3];
        usSensor.continuous();
        head.setSpeed(900); // bring up the speed to make it go as fast as possible

        // rotate top motor by 5185 and take right reading
        head.rotate(5185);
        Sound.pause(20);
        surr[0] = usSensor.getDistance();
        LCD.drawInt(surr[0], 0, 0);
        
        // rotate top motor by -5185 and take forward reading
        head.rotate(-5185);
        Sound.pause(20);
        surr[1] = usSensor.getDistance();
        LCD.drawInt(surr[1], 0, 1);

        // rotate top motor by -5185 and take left reading
        head.rotate(-5185);
        Sound.pause(20);
        surr[2] = usSensor.getDistance();
        LCD.drawInt(surr[2], 0, 2);

        // rotate top motor by 5185 to reset the US position
        head.rotate(5185);

        usSensor.ping();
        return surr;
    }

    /**
     * Method to determine if robot is at end of maze
     * NOTE: calibrate light sensor with setHigh() and setLow() before using this method
     *
     * @return - true if on a black space, false otherwise
     */
    public boolean onBlack()
    {
        lightSensor.setFloodlight(true); // turn on LED
        int reading = lightSensor.readValue(); // get the value
        // white represents a low value, black represents high value
        if(reading < 50)
            return true;
        else
            return false;
    }

    public void calibrateLightSensor()
    {
        
    }
    
}
